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Abstract. In this paper we study a discrete variational optimal control prob- 
lem for the rigid body. The cost to be minimized is the external torque applied 
to move the rigid body from an initial condition to a pre-specified terminal 
condition. Instead of discretizing the equations of motion, we use the discrete 
equations obtained from the discrete Lagrange— d'Alembert principle, a process 
that better approximates the equations of motion. Within the discrete-time 
setting, these two approaches are not equivalent in general. The kinemat- 
ics are discretized using a natural Lie-algebraic formulation that guarantees 
that the flow remains on the Lie group S0(3) and its algebra so(3). We use 
Lagrange's method for constrained problems in the calculus of variations to 
derive the discrete-time necessary conditions. We give a numerical example 
for a three-dimensional rigid body maneuver. 

1. Introduction 

This paper deals with a structure-preserving computational approach to the 
optimal control problem of minimizing the control effort necessary to perform an 
attitude transfer from an initial state to a prescribed final state, in the absence 
of a potential field. The configuration of the rigid body is given by the rotation 
matrix from the body frame to the spatial frame, which is an element of the group 
of orientation-preserving isometrics in R'^ . The state of the rigid body is described 
by the rotation matrix and its angular velocity. 

To motivate the computational approach we adopt in the discrete-time case, we 
first revisit the variational continuous-time optimal control problem. The continuous- 
time extremal solutions to this optimal control problem have certain special fea- 
tures, since they arise from variational principles. General numerical integration 
methods, including the popular Runge-Kutta schemes, typically preserve neither 
first integrals nor the characteristics of the configuration space. Geometric integra- 
tors are the class of numerical integration schemes that preserve such properties, 
and a good survey can be found in [5]. Techniques particular to Hamiltonian sys- 
tems are also discussed in [16] and [24]. 

Our approach to discretizing the optimal control problem is in contrast to tradi- 
tional techniques such as collocation, wherein the continuous equations of motion 
are imposed as constraints at a set of collocation points. In our approach, modeled 
after [11]. the discrete equations of motion are derived from a discrete variational 
principle, and this induces constraints on the configuration at each discrete time 
step. 

This approach yields discrete dynamics that are more faithful to the continuous 
equations of motion, and consequently yields more accurate solutions to the optimal 
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control problem that is being approximated. This feature is extremely important 
in computing accurate (sub)optimal trajectories for long-term spacecraft attitude 
maneuvers. For example, in [9], the authors propose an imaging spacecraft forma- 
tion design that requires a continuous attitude maneuver over a period of 77 days 
in a low Earth orbit. Hence, attitude maneuver has to be very accurate to meet 
tight imaging constraints over long time ranges. 

While the discrete optimal control method presented here is illustrated using the 
Lie group S0(3) of rotation matrices, and its corresponding Lie algebra so(3) of 
skew-symmetric matrices, we have derived the method with sufficient generality to 
address the problem of optimal control on arbitrary Lie groups with the drift vector 
field given by geodesic flow on the group, and it therefore widely applicable. For 
example, in inter-planetary orbit transfers (see, for example, [1]), one is interested 
in computing optimal or suboptimal trajectories on the group of rigid body motions 
SE(3) with a high degree of accuracy. Similar requirements also apply to the control 
of quantum systems. For example, efficient construction of quantum gates is a 
problem on the unitary Lie group SU(A^). This is an optimal control problem, 
where one wishes to steer the identity operator to the desired unitary operator 
(see, for example, [13] and [23]). 

Moreover, an important feature of the way we discretize the optimal control 
problem is that it is S0(3)-equivariant. The S0(3)-equivariance of our numerical 
method is desirable, since it ensures that our results do not depend on the choice of 
coordinates and coordinate frames. This is in contrast to methods based on coor- 
dinatizing the rotation group using quaternions, (modified) Rodrigues parameters, 
and Euler angles, as given in the survey [26]. Even if the optimal cost function is 
S0(3)-invariant, as in [25], the use of generalized coordinates imposes constraints 
on the attitude kinematics. 

For the purpose of numerical simulation, the corresponding discrete optimal con- 
trol problem is posed on the discrete state space as a two stage discrete variational 
problem. In the first step, we derive the discrete dynamics for the rigid body in 
the context of discrete variational mechanics [20] . This is achieved by considering 
the discrete Lagrange-d'Alembert variational principle [12] in combination with 
essential ideas from Lie group methods [10], which yields a Lie group variational 
integrator [17]. This integrator explicitly preserves the Lie group structure of the 
configuration space, and is similar to the integrators introduced in [14] for a rigid 
body in an external field, and in [15] for full body dynamics. These discrete equa- 
tions are then imposed as constraints to be satisfied by the extremal solutions to 
the discrete optimal control problem, and we obtain the discrete extremal solutions 
in terms of the given terminal states. 

The paper is organized as follows. As motivation, in Section 2, we study the 
minimum control effort optimal control problem in continuous-time. In Section 3, 
we study the corresponding discrete-time optimal control problem. In Section 3.1 
we state the optimal control problem and describe our approach. In Section 3.2, 
we derive the discrete-time equations of motion for the rigid body starting with the 
discrete Lagrange-d'Alembert principle. These equations are used in Section 3.3 
to obtain the solution to the discrete optimal control problem. In Section 5, we 
describe an algorithm for solving the general nonlinear, implicit necessary condi- 
tions for 50(3) and give numerical examples for rcst-to-rest and slew-up spacecraft 
maneuvers. 
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2. CONTINUOUS-TlME RESULTS 

2.1. Problem Formulation. In this paper, the natural pairing between so* (3) 
and so(3) is denoted by (•,•). Let <C •, • ^ and <C •,• ^* denote the standard 
(induced by the KiUing form) inner product on so (3) and SO* (3), respectively. The 
inner product ^ •, • 3>* is naturally induced from the standard norm <^ ^,u} 3>= 
-|Tr(4^u;), for all e so(3), through 

(1) = <^,'^>, 

where (p — lj^ ^ so* (3) and rj — ^ so* (3), with ^,u) G so(3) and b and fl are the 
musical isomorphisms with respect to the standard metric <C •, • On so(3), these 
isomorphisms correspond to the transpose operation. That is, we have (p = uj"^ and 

v = e. 

Let J : so(3) so* (3) be the positive definite inertia operator. It can be shown 
that 

(2) (J(0,a;) = (JM,0- 

On so (3); J is given by J(^) = + ^J, where J is a positive definite symmetric 
matrix (see, for example, [3, 8]). Moreover, we also have 3{ri^y — [Jvi^ + rf- J)"^ = 
J(j7), which is an abuse of notation since -q G so*(3). For the sake of generality and 
mathematical precision we will use the general definitions, though it helps to keep 
the above identifications for so(3) in mind. 

In this section we review some continuous-time optimal control results using 
a simple optimal control example on S0(3). The problem we consider is that of 
minimizing the norm squared of the control torque r S SO* (3) applied to rotate 
a rigid body subject to the Lagrange-d'Alembert principle for the rigid body^ 
whose configuration is given by R G S0(3) and body angular velocity is given by 
fl € so(3). We require that the system evolve from an initial state (Ro,rJo) to a 
final state (Rt, ^t) at a fixed terminal time T. 

Before proceeding with a statement of the optimal control problem, we first define 
variations of the rigid body configuration R and its velocity il. Let W(t) € so(3) 
be the variation vector field associated with a curve R(<) on S0(3) [2, 28]. The 
vector field W(t) satisfies 

suit) = RW e TR,(t)S0(3), W(0) = 0, W(r) = 0, 

where ^R is defined by ^R(i) = d'Rf_{t) / de\ with Utit) '■= R(i, e) is the vari- 
ation of the curve R(t) that satisfies R(i, 0) = R(t). The variation in the velocity 
vector field is denoted dfl. For a deeper understanding of variations of general 
vector fields, see for example the treatment in [7] . 

We now state the minimum control effort optimal control problem. 



This is equivalent to constraining the problem to satisfy the rigid body equations of motion 
given by equations (7). However, for the sake of generality that will be appreciated in the discrete- 
time problem, wc choose to treat the Lagrange-d'Alembert principle as the constraint as opposed 
to the rigid body equations of motion. Both are equivalent in the continuous-time case but are 
generally not equivalent in the discrete-time case. 
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Problem 2.1. Minimize 

(3) J = ^ / <r,x>, di 
subject to 

(1) satisfying Lagrange-d'Alembert principle; 

(4) s r U'im,^)dt+ r {T,w}dt=o, 

Jo ^ Jo 
for a variation vector field W(i), and subject to 11 = Ril, 

(2) and the boundary conditions 

R(0) = Ro, n{0) = ^0, 

(5) R(T) = Rt, n{T) ^ nr. 

We now show that the constraint of satisfying the Lagrange-d'Alembert princi- 
ple leads to the following problem formulation, where the rigid body equations of 
motion replace the Lagrange-d'Alembert principle. 

Problem 2.2. Minimize 

1 

(6) J = - / < r,x >, di 

subject to 

(1) the dynamics 

(7) R = Rn 

M = ad^M + T = [M, rj] + T, 

where M = J(J1) G so* (3) is the momentum, 

(2) and the boundary conditions 

R(0) = Ro, n(0) = flo, 

(8) R(T) = Rr, n{T) = fir- 

In the above, ad* is the dual of the adjoint representation, ad, of so(3) and is 
given by adjry = -[I,*?] e so*(3), for all | € so(3) and e so*(3). Recall that the 
bracket is defined by uj] ~ ^a; — o;^. 

2.2. The Lagrange d'Alembert Principle and the Rigid Body Equations 

of Motion. In this section we derive the forced rigid body equations of motion 
(equations (7)) from the Lagrange-d'Alembert principle. We begin by appending 
the constraint R = RJl to the Lagrangian 

= s Q (j(rj),rj) + ^A,R-iR- dt 

T 

(x,W) dt, 

where A e 50* (3) is a Lagrange multiplier. Taking variations, we obtain 
= ^ (^^-A - [r2, A] +r,W^ + (-A + J(f2) ,^r2)^dt 
+ [(A,W(i))]^ 
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The term outside the integral vanishes by virtue of the boundary conditions on 
W(i). Since Sfl and W are arbitrary and independent, we must have ( — A + 
J (r2) , (50) = and hence A = J (Jl) which is the body angular momentum. For 
the remainder of this section, we set M = A = J(r2). The first term in the 
integrand gives us the forced second order dynamics of the rigid body which is 

M = [M,n] + T. 

This completes the proof that the Problem (2.1) is equivalent to Problem (2.2). 

A Direct Approach. We now give a direct derivation that does not involve 
the use of Lagrange multipliers. This approach can be found in Section 13.5 in [21]. 
First, we take variations of the kinematic condition ft — R~^R to obtain dCl — 
-R-i ((5R) R-iR + R-i (^StCj . As defined previously, we have W = R^MR and, 

therefore, W = -R-^RR-^^R + R-^^R = -OW + R^MR, since ^R = §dR 
(see for example [22], page 52). Hence, we have 

(9) = -Wri + rJW + W = adnW + w. 
Taking variations of the Lagrange- d'Alembcrt principle we obtain 

[ (J(fl),^O) + (T,W)dt = 0. 
Jq 

Using the variation in equation (9) and integrating by parts, we obtain 

= 1^ (-M + ad* M + r, w) dt + [(J (O) , W(t))]^ , 
where we have used the identity 

(10) (»7,ad^0 = (ad:»7,0 , V e so*{3), a;,^ e so(3). 

This gives the desired result, with M = J (17). 

In Section 2.3, we demonstrate how the necessary conditions for Problem (2.2) 
are derived using a variational approach. 

2.3. Continuous-Time Variational Optimal Control Problem. A direct vari- 
ational approach is used here to obtain the differential equation that satisfies the 
optimal control Problem (2.2). 

A Second Order Direct Approach. "Second order" is used here to refiect the 
fact that we now study variations of second order dynamical equations as opposed 
to the kinematic direct approach studied in Section 2.2. We now give the resulting 
necessary conditions using a direct approach as in [21]. We already computed the 
variations of R and H. These were as follows: (5R = RW and Sfl = adfjW + W. 
We now compute the variation of M with the goal of obtaining the proper variations 
for r: 

6M = 3 (sti^ = J (^■^sn + n{w,fi)n^ , 

where TZ is the curvature tensor on SO (3). The curvature tensor TZ arises due to 
the identity (see [22], page 52) 

11- -M-^ 
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where Y e TS0(3) is any vector field along the curve R(i) G S0(3). Taking 
variations of M = ad^M + t, we obtain = adJ^M + ad^^M + St. We now 
have the desired variation in t: 

(11) St = J(7^(W,^2)0) + -^J(^^^)-ad^J^M-ad^^M. 

Taking variations of the cost functional (6) we obtain: 

SJ = 1^ (^(J(^) - ad^ (J(^)) + ^ - ^ (ad^M) 

+ [n (J(^)«, fl) n] ' + ad^ad*M - ad^r?, W))dt, 



where ^ = t" G so(3) and -q ~ J (adfj^) G so* (3). In obtaining the above expres- 
sion, we have used integration by parts and the boundary conditions (8), equations 
(9) and (11), and the identities (1), (2) and (10). Hence, we have the following 
theorem. 

Theorem 2.1. The necessary optimality conditions for the problem of minimizing 
(6) subject to the dynamics (7) and the boundary conditions (8) are given by the 
single fourth ordcP differential equation 

= J(^-)-ad^(J(?))+r;-^(ad*M) 

n ((J(^))» , ' + ad?2 (ad*M) - ad^r,, 

as well as the equations (7) and the boundary conditions (8), where (; and r) are as 
defined above. 

Note that for a compact semi-simple Lie group G with Lie algebra g, the curvature 
tensor, with respect to a bi-invariant metric, is given by (see [22]): 

(12) 7^(X,Y)Z = iadadxvZ, 

for all X, Y, Z G g. 

Using a Lagrange multiplier approach as in Section 2.2, we may show that the 
result of Theorem 2.1 is equivalent to the following theorem. 

Theorem 2.2. The necessary optimality conditions for the problem of minimizing 
(6) subject to the dynamics (7) and the boundary conditions (8) are given by 



2 



(13) Ai = 7e j(A2)",n n 



adJ^Ai 

A2 = -J-i(Ai)-adoA2 + J-i(adX^M) 

as well as the equations (7) and the boundary conditions (8), where the Lagrange 
multipliers Ai G SO* (3), A2 G so (3) correspond to the kinematic and dynamics 
constraints (7), respectively. 



^Second order in -r and fourth order in R. 
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Remark 2.1. Note that the equations of motion that arise from the Lagrange- 
d'Alemhert principle are used to define the dynamic constraints. So, in effect, we 
are minimizing J subject to satisfying the Lagrange-d'Alembert principle for the 
rigid body. Analogously, the discrete version of the Lagrange-d'Alembert principle 
will be used to derive the discrete equations of motion in the discrete optimal control 
problem to be studied in Section 3.3. This view is in line with the approach in [11] in 
that we do not discretize the equations of motion directly, but, instead, we discretize 
the Lagrange- dAlembert principle. These two approaches are not equivalent in 
general. 

Corollary 2.1. The necessary optimality conditions of Theorem 2.1 are equivalent 
to the necessary conditions of Theorem 2.2. 

Proof. In Theorem 2.2, differentiate A2 once and then use aU three differential 
equations to replace Ai and A2 with expressions involving only r, M and fJ. □ 

3. Discrete-Time Results 

3.1. Problem Formulation. In this section we give the discrete version of the 
problem introduced in Section 2.1. So, we consider minimizing the norm squared 
of the control torque r subject to satisfaction of the discrete Lagrange-d'Alembert 
principle for the rigid body whose configuration and body angular velocity at time 
step tk are given by R/c € S0(3) and Vlk G so(3), respectively. The kinematic 
constraint may be expressed as 

(14) Rfe+i =RfeCxp(/iflfe) =Rfcgfe, 

where h is the integration time step, exp : so(3) S0(3) is the exponential map and 
gfe = exp(/ir2fc). The boundary conditions are given by (Rq, ^Jq) and (R^, r2J^_j^), 
where to = and N = T/h is such that t^ = T. 

More generally, one considers the ansatz Rfe+i = Rfc exp where is 

an interpolatory curve in so (3) parameterized by the angular velocity at internal 
nodal points. This allows one to construct Lie group variational integrators of 
arbitrarily high order [17]. To simplify the subsequent treatment, we adopt (14) as 
the kinematic constraint, which yields a first-order accurate Lie symplectic Euler 
method, which will nevertheless have effective order two as it is symplectically 
conjugate to the second-order accurate Lie Stormer-Verlet method (see, §4). 

The reason we constrain fl aXt = h{N—l) instead of at t = hN will become clear 
when we derive the discrete equations of motion in Section 3.2. A simple expla- 
nation for this is that a constraint on flk G so(3) corresponds, by left translations 
to a constraint on R^ G Trj.S0(3). In turn, in the discrete setting and depending 
on the choice of discretization, this corresponds to a constraint on the neighboring 
discrete points . . . , Rfc_2, Rfe-i, Rfe+i, Rfe+2, ■ • ■• With our choice of discretization 
(equation (14)), this corresponds to constraints on R^ and Rfc+i. Hence, to ensure 
that the effect of the terminal constraint on 17 is correctly accounted for, the con- 
straint must be imposed on flN-i, which entails some constraints on variations at 
both Rjv-i and Rat. We will return to this point later in the paper. 

The discrete kinematic constraint ensures that the sequence Rfc stays on the 
rotation group, since the exponential of the angular velocity matrix fi^, which is 
in the algebra so(3), is a rotation matrix, and the rotation group is closed under 
matrix multiplication. This is natural to do in the context of discrete variational 
numerical solvers (for both initial value and two point boundary value problems). 
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Following the methodology of [11], we have the following optimal control prob- 
lem. 

Problem 3.1. Minimize 

N 1 

(15) J = ^-<Tfc,rfc>,, 

k=0 

subject to 

(1) satisfying the discrete Lagrange-d'Alcmbcrt principle; 

N~l N 

(16) SY,-{J{^lk),n,)+Y,{rk,W,)^0, 

k=0 k=0 

subject to Ro = Ro, Rw ~ R^y and Hk+i = R-fcgfe, k = 0, 1, . . . , — 1, 
where is the variation vector field at time step tk satisfying ^Rfc = 

(2) and the boundary conditions 

(17) Rjv = RjV) ^^Af-i = ^iv-i- 

In Problem 3.1, the discrete Lagrange-d'Alembert principle is used to derive the 
equations of motion for the rigid body with initial and terminal configuration con- 
straints. Hence, we get a two point boundary value problem. The full configuration 
and velocity boundary conditions come into the picture when we study the optimal 
control problem. We will show that the constraint of satisfying the Lagrange- 
d'Alembert principle in Problem 3.1 leads to the following problem formulation, 
where the discrete rigid body equations of motion replace the Lagrange-d'Alembert 
principle constraint. Only when addressing the following optimal control problem 
will we need to include the velocity boundary conditions in the derivation. 

Problem 3.2. Minimize 

(18) J = Y^-<:Tk,Tk'><, 

k=0 

subject to 

(1) the discrete dynamics 

Rfc+i =Rfegfe, fc = 0,...,Af-l 

Mk = Adl^{hTk + Mk-i), fc = 1,...,A^-1, 

(19) Mfe = J(flfc), fc = 0,...,A-l, 

(2) and the boundary conditions 

(20) Rtv = RjVj = ^N-l- 

Regarding terminal velocity conditions, note that in the second of equations (19) 
if we let fc = A^ we find that JIat appears in the equation. A constraint on fljv 
dictates constraints at the points Rat and Rjv-i-i through the first equation in (19). 
Since we only consider time points up to t = Nh, we can not allow A: = A^ in the 
second of equations (19) and hence our terminal velocity constraints arc posed in 
terms of flN-i instead of riw. 



GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 9 



As mentioned above, is a variation vector field associated with the perturbed 
group element R|. Likewise, we need to define a variation vector field associated 
with the element = cxp(/ir2fe). First, let the perturbed variable be defined 

by 



(21) 
where 



Sl = gfcexp(eMr2fc), 



de 



e=0 



Note that gi;|g_Q = gfe as desired. Moreover, we have 

(22) ^gfe = gkih6n^)exp{ehdnk)\^^^ = hskSn^. 

This will be needed later when taking variations. 



3.2. The Discrete Lagrange d'Alembert Principle and the Rigid Body 
Equations of Motion. In this section we derive the discrete forced rigid body 
equations of motion (equations (19)) starting with the discrete Lagrange-d'Alembert 
principle. We begin by rewriting the kinematic constraint as exp~^ (R'J^ Rfe+i) = 
hflk, which is an expression on the Lie algebra so (3), and appending it to the 
Lagrangian 



= ^(rfe,W,)+^^ ( -(J(f^fc),nA 

fe=0 k=0 



Mfc, i exp-i (R^iRfe+i) - n,^ ^ , 



where Mfe G so*(3), A; = 0, 1, ... , N— 1, are Lagrange multipliers. These multipliers 
enforce the kinematic discrete equations. We could have added additional terms 
to enforce the configuration boundary conditions, allowing for Wq and Wjv to be 
arbitrary and non-zero. Instead, we elect to enforce the constraints by requiring 
Wq = and SS},q = 0. These two approaches are of course equivalent. 

Taking variations of exp^^ (R^T^Rfc+i) ~flk = is equivalent to taking vari- 
ations of the original expression R^^R^+i = cxp (/lOfc), and is easier to compute 
since it as an expression over the Lie algebra. Once the variations are computed, 
one can easily obtain the Lie algebra-equivalent of the variations as follows. First 
take variations of the kinematics (14) to get — R^T^ (^Rfc) R^T^Rfc+i + Rjr^5Rfc+i — 
hgk ■ Sftk, which is equivalent to -W^g^ + g^W^+i = hgkS^k, or 



(23) SQk - I 

h 



-Adg-iWfc+Wfc+i 



Note that this is an expression over the Lie algebra so(3). 
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After simple algebraic and re-indexing operations, the Lagrange-d'Alembert 
principle gives 



0= /to - iAd*-iMo,Wo 

\ /l So 



Tjv + ^Mat-i, Wat 
h 



J2 {Jin,)-Mk,dnk) 

k=Q 

'V-l / 1 1 

(rfc--Ad*-iMfc + -Mfe_i,Wfc 



k=l 



By the boundary conditions Ro = and Rat = R^, we have Wq = and 
Wat = 0. Since Sflk, k = 0, . . . , N— 1, and W^, k = 1, . . . , N— 1, are arbitrary and 
independent, then the Lagrange-d'Alembert principle requires that the equations 
(19) hold true. The variables M^, A: = 0, . . . , iV — 1, are of course nothing but the 
discrete angular momentum of the rigid body. The equations (19) can be viewed 
in two ways. The first is to consider the two point boundary value problem where 
we retain the terminal condition on Ryv- In this case a (constrained) variety of a 
combination of control torques Tk, k ^ 0, . . . , N , and initial velocity conditions flo 
can be chosen to drive the rigid body from the initial condition Rq to the terminal 
condition Rat. The second view is to treat it as an initial value problem by ignoring 
any terminal configuration constraints. In this case Wat and any combination 
of control torques Tk, k = 0, . . . , N, and initial velocity conditions flo can be chosen 
freely. 

A Direct Approach. Taking direct variations of the cost functional we obtain 
0= ^To-iAd*-iJ(llo),Wo 

+ E ( ^fc - ^Ad;-i J (fifc) + -J (J^fc_i) , W^ 

k=l ^ 

where we have used equation (23). This gives the same equations of motion as 
those in equation (19). 

Simulation Results. To test our results, we re-write the discrete equations (19) for 
the subgroup S0(2). For S0(2) we have 



(24) 



and 



Rfc 



cos Wfe 

sin dk 



— sm til; 
cos 0k 



Qk = 





L^k 



-^k 





(25) exp(f2fc) = 
The inertia operation is simply given by 

(26) J(f2fc) 



cos uJk ~ sm ujk 
sin UJk cos UJk 



-lujk 
luJk 
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where / is the mass moment of inertia of the body about the out-of-plane axis. One 
can check that Adcxp(oj)€ = € £ind that Adl^p(^^^^ri ~ rj, for aU ^, uj E so(2) and 
r] e 50*(2). 

Then the equations (19) (treated as an initial value problem) are given for S0(2) 

by 

Ok+i = Ok + hLUk, k = 0,...,N-l 

(27) ojk = jTk+ujk-i, k^l,...,N-l 

in addition to the initial conditions 6q = 6^, ujq ^ luq. 

To verify the accuracy of our numerical computation, we give the corresponding 
continuous-time equations of motion for the planar rigid body on S0(2) using equa- 
tions (7). The Lie bracket on S0(2) is identically equal to zero. Hence, one can 
check that the equations (7) are given hy = cu, uj ^ j, where 6, lu and r arc the 
continuous time angular position, velocity and torque, respectively. We integrate 
the equations using the torque T{t) = sin(^), t € [0,T]. We use the following 
parameters for our simulations: T = 10, / = 1, d{Q) = 3, i^{Q) = 4 and we try three 
different time steps corresponding to = 1000, 1500, 2000. The error between the 
continuous- and discrete-time values of 9 and lu are given in Figure (1). Note that 
the accuracy of the simulation improves with increasing N. 

Remark 3.1. Note that the discrete-time equations (27) correspond to the Euler 
approximation for the equations of motion. This is a check that our method returns 
something familiar for a simple example as the planar rigid body. However, we 
emphasize that on S0(3) the discretization will not necessarily be equivalent to any 
of the classical discretization schemes. The discretization will generally result in a 
set of nonlinear implicit algebraic equations. 

3.3. Discrete-Time Variational Optimal Control Problem. We now address 
Problem 3.2 by first forming the appended cost functional: 

^ 1 

fc=0 
N-1 

+ J2 (K, -h^k + exp-i (R-iRfe+i)) 

A:=0 

N-l 

+ X (Mfe - Ad*^ {hTk + Mk-i) , A^) . 
fe=i 

Writing Mfc = Ad*^ (/ix^ + Mfe_i) as Mfc = ^ (/ir^ Mfc_i) gfc and taking 
variations of this expression, we obtain 

SMk = -hSnkSk' {hTk + Mk-i)gk 
+g-'ihSTk + 5Mk-i)gk 
+hg-^{hTk + Mk-i)SkSflk 

= -hdnkMk + Ad;^ {hdTk + J (snk-i)) 

+hMkSnk 

= AdlJhdTk + 3{dnk-i)) + h[Mk,snk]. 
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Figure 1. Error dynamics on S0(2). 

In obtaining this expression we used the facts that 5g,k = hgkSClk and 6 (g^ 
— hSflkg^^ ■ The latter equality is obtained as follows. Taking variations of (g^.) 
I, we obtain 

which implies that 

which is the desired result. 
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We now also consider the velocity boundary conditions Jig = J^q and r2Ar_i = 
r2^_]^. Note that variations in Srik directly induce variations in W^+i. In par- 
ticular, ii k — and we have the initial constraints Rq = Rq and JIq = ^q, then 
Wq = and 6fto = 0. Using these two equations in equation (23), we find that 

(28) Wi = 0. 

At fc = A^, A^ — 1, the constraints R^r = Rjv ^^'^ ^n-i = ^jv-i imply that Wjy — 
and ^r2jv-i = 0. Using these two equations in equation (23), we find that 

(29) Wat^i^O. 

The observations stated in equations (28) and (29) are equivalent to having 

(30) Ri = R5 exp (hn*„) , Ryv-i = R^ cxp . 
Taking variations of the cost functional, we obtain 

N 



k=Q 
N-1 I 

+ J2{Al-snk + - 



k=0 ^ 

N-1 



-Adg-iWfc+Wfc+i 



J2 (J i^^k) - Ad^^ {hSTk + 3 (snk-i)) 



k=l 



-h[Mk,Snk],Al), 



where we have replaced ^M^ with J (Sflk). Collecting terms, setting SJ' to zero, 
and using the conditions Wq = Wi = W^r-i = Wat = SQq = Sfl^^i = and 
the fact that J(-) is self-adjoint, we obtain the following theorem. 

Theorem 3.1. The necessary optimality conditions for the discrete Problem 3.2 
are 

Rfc+i = Rfegfe, fc = l,...,7V-2 
Mfe = Ad;^(/irfe + Mfe_i), A: = l,...,iV-l 
= Ai._i-Ad;-iAi, fc = 2,...,iV-2 
(31) -Ai+J(A2)_j(Adg,^,A2^i) 

+h[mk,Ai] , k = i,...,N-2 

Tk = h{M^,Al)\ k=\,...,N-l 
Mfc = J (fife), A: = 0,...,Ar-l, 

and the boundary conditions 

R-o = Rq, R-i = R-oSo' ^0 = J^o 

RjV = RjV; = R-AT (SAf-l) ; ^N-l = -1 

To = TN = 0, 

where gg = exp(/if2g) and = exp (/ifJ^ j^). 
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A Second Order Direct Approach. Analogous to the direct approach in continuous 
time, here we derive the necessary optimahty conditions in a form that does not 
involve the use of Lagrange multipliers. Using equation (23) and taking variation 
of the second of equations (19), wc obtain 



Ad:-J -^^J(Wfc+i-Ad -iWfe 



Wfc+i-Ad -iWfc,J(Ofe) 



(32) 



1 

"h 



for fc = 1, . . . , TV — 1. Taking variations of the cost functional (18) and substituting 
from equation (32) one obtains after a tedious but straight forward computation 
an expression for 5j7 in terms of ^r^: 

JV-l 



k=l 
1 



(Ad 



1 



J (Wfe+i- Adg-iW 



+ 



h 



Wfe+i-Ad iWfe,J(f2fe) 



j(Wfe-Ad Wfc_i),r«) 



When SJ^ is equated to zero (and after some algebraic rearrangement), one can 
obtain the boundary conditions on Tq,ti,tn^i,tn from the resulting equations 
below: 



To 




= 



--Ad*-i 
h Si 



= 



-Ad*-.J(^Ad^-.x» 
J(fli),Adg-i (t«)" 
i,(j(rV.,)+Ad*-.^j(Ad^-.^.|,_, 

j(rj^_i),Ad, 



--Ad*-i 



N-1 



TN 







as well as discrete evolution equations that arc written in algebraic nonlinear form 
as: 

1 



= - 



/l2 



Adl-iJ (Adg-iT« 



(33) 

for fc 2, . . . , TV - 2. 



J I Ad! 

1 

"h 



J(f^fc),Ad -1 {tI 
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The following section shows that while our discrete approximation (14) is for- 
mally first-order accurate, it is symplcctically equivalent to the second-order accu- 
rate Stormer-Verlet method, and hence has effective order two. 

4. Lie Symplectic Euler and Symplectic Equivalence 

Notice that the discrete Lagrangian adopted in our paper is obtained by approx- 
imating the velocity as a constant over the timestep h, and by approximating the 
integral in time by J*^ f{t)dt « (t2 — ti)f{ti). In the Lie group setting, the constant 
angular velocity approximation corresponds to the condition, 

Rfe+i = Rfc expihQk) 

or equivalently, 

When we let G = R", and we adopt the notation (q, v) S TM", we obtain, 

qfc+i - Qfc 

which is a usual finite-difference approximation for the velocity. Consider then a 
Lagrangian of the form, 

L(q,v) = VMv-y(q). 

Approximating the action integral from to h using a constant velocity approxi- 
mation and a quadrature formula, yields. 



qfe+i - qfc 



We then choose as our discrete Lagrangian, 



-^<i(qfe,qfc+i) = /ii(qfe, ) = h 



h 

The discrete Euler-Lagrange equations, 

£'2id(qfc-i,qfe) + L'iLd(qfc,qfc+i) = 0, 

yields, 

which induces an implicit update map (qfc_i,qfc) i— > (qfe,qfc+i). To obtain the 
corresponding Hamiltonian update map, we push-forward this algorithm to T*Q 
by using the discrete fiber derivative Fi^ : Q x Q ^ T*Q, which takes (q^, q^+i) ^ 
(qfe+i,L'2id(qfc,qfe+i))- In particular, we have that, 

Pfc+i = L'2id(qfe,qfe+i) = ui^^^^^j-^^, 

which implies 

(34) qfe+i =qfc + Mf-Vfe+i. 

This allows us to rewrite the discrete Euler-Lagrange equations as, 

dV 
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or equivalently, 

dV 

(35) pfc+i = pfe - /i— (q/c). 

Now, (34) and (35) are precisely the symplectic Euler method apphed to the cor- 
responding Hamiltonian vector field, as we shall see. 
The corresponding Hamiltonian is given by, 

i^(q,p) = ^p^^/"'p + ^(q)- 

Hamilton's equations yield, 

q 
p 

The symplectic Euler method has the form, 

q/c+i = qfc + /iq(qfc,Pfc+i), 
Pfe+1 = Pfc + /ip(qfc,Pfc+i), 

which yields, 

qfc+i = qfe + hhr^-pk+i, 
Pfc+i = Pfc + h --^(qfc) 




dq 

which is precisely what we obtained in (34) and (35). This demonstrates that our 
method is the generalization of the symplectic Euler method to Lie groups, which 
has important numerical consequences. While symplectic Euler is formally first- 
order accurate, it is symplectically equivalent [27, 18] to the second-order accurate 
Stormer-Verlet method [6]. This means that one can obtain the Stormer-Verlet 
method FsY by conjugating the symplectic Euler method Fe with a symplectic 
transformation T, 

In particular, numerical trajectories of symplectic Euler will shadow numerical tra- 
jectories obtained using Stormer-Verlet. Consider the implications of this sym- 
plectic equivalence for our discrete optimal control problem. Let the boundary 
conditions be specified by qo , qAr , and assume that we use Stormer-Verlet to prop- 
agate the solution, then the boundary condition is expressed as, qAr = -Fsvqo = 
{TFET-^)^qo = TF^T-iqo, which is equivalent to g^v = T-^(In = F^T-^qo 
F^qo. This implies that if we preprocess the boundary conditions qo,qAr, to ob- 
tain go = T^^qQ,qj^ = T^^q^r, we could use symplectic Euler at the internal 
stages to propagate the states and costates, and then postprocess them to obtain 
the trajectory one would have obtained by using Stormer-Verlet. 

In practice, the shadowing result imparts the symplectic Euler method with the 
same desirable qualitative properties as Stormer-Verlet, and it is not necessary 
to postprocess the numerical solutions in order to achieve accurate results. Since 
on an appropriate choice of charts, our Lie symplectic Euler method reduces to 
symplectic Euler in coordinates, it follows that there is a corresponding second- 
order Lie Stormer-Verlet method that our method is symplectically equivalent to, 
and in particular, our method has effective order two. 
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5. Numerical Approach and Results 

The multiplier- free version of the first-order optimality equations, equation (33), 
in combination with the boundary conditions, 

R-O = R-Oi R-W — R-jV' ^0 = J^O' ^^^"^ ^N-l = J^jV-U 

leave the torques Ti, . . . ,tn~i, and the angular velocities fli, . . . ,^^^2 as un- 
knowns. By substituting the relations gfe = ex.p{hftk); ~ 3{flk), we can 
rewrite the necessary conditions (33) as follows, 

0--)^(j(^*)-Ad:.p(-,o.)J(^Li) 

- J(Adcxp(_hr!fc_i)Tfe_i 

+ Ad*xp(-ftf2j^)J(Adcxp(-hf2fc 

- ^ (^Ad*xp(_/jn J J(nfc), Adexp(-/if2t,)(T j.) 

1 r J 

- -^J(f2fc_i),Adexp(-;jS7,_i)(-rJ;_i) 

where k = 2, . . . ,iV — 2, and the discrete evolution equations, given by line 2 of 
(31), can be written as 

=j(j^fe) - Adi^p^i^^jhTk + Jin^-i)), 

where fc = 1, . . . , iV — 1. In addition, we use the boundary conditions on Rq and 
Rat, together with the update step given by line 1 of (31) to give the last constraint, 

= log ( R^^Ro exp(/ir2o) • ■ • cxp(/iO 



^ xv,oexp(_AtiJoj • ■ -exp^/iiijv-i, 

where log is the logarithm map on SO (3). 

Note that while we use the direct variational approach to obtain the discrete 
extremal solutions, an alternate way to obtain the discrete extremal solutions would 
be to use Pontryagin's maximum principle. In particular, Bonnans and Laurent- 
Varin [4] show that these two approaches are equivalent in the context of symplectic 
partitioned Runge-Kutta schemes. 

At this point it should be noted that one important advantage of the man- 
ner in which we have discretized the optimal control problem is that it is S0(3)- 
equivariant. This is to say that if we rotated all the boundary conditions by a fixed 
rotation matrix, and solved the resulting discrete optimal control problem, the so- 
lution we would obtain would simply be the rotation of the solution of the original 
problem. This can be seen quite clearly from the fact that the discrete problem is 
expressed in terms of body coordinates, both in terms of body angular velocities 
and body forces. In addition, the initial and final attitudes Rq and Rjv only enter 
in the last equation as a relative rotation. 

The S0(3)-equivariance of our numerical method is desirable, since it ensures 
that our results do not depend on the choice of coordinate frames. This is in 
contrast to methods based on coordinatizing the rotation group using quaternions 
and Euler angles. 
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Each of the equations above take values in so (3). Consider the Lie algebra 
isomorphism between M.^ and 5o(3) given by the hat map, 










-V3 


2 


V = {V1,V2,V3) 1- 


-> V = 







-Vl 






-V2 


Vl 






which maps 3-vectors to 3 x 3 skew-symmetric matrices. In particular, we have the 
following identities, 

[u, v] = (u X v)', AdAV = (Av)". 



Furthermore, we identify so(3)* with K.^ by the usual dot product, that is to say if 
n, V e IR-^, then (11, v) = 11 • v. With this identification, we have that Ad^-iII = 
An. Using the identities above, we write the necessary conditions using matrix- 
vector products and cross products. Then, each of the equations can be interpreted 
as 3-vector valued functions, and the system of equations can be considered as 
a 3(2A^ — 3)-vector valued function, which is precisely the dimensionality of the 
unknowns. This reduces the discrete optimal problem to a nonlinear root finding 
problem. 

The nonlinear system of equations was solved in MATLAB using the f solve 
routine, where the Jacobian is constructed column by column, and the k-th column 
is computed using the following approximation [19], 

dF , ^ 1 , r^, 

— (x) = -Im[F(x + zeefc)], 

OXk e 

where i = >/— T, efc is a basis vector in the Xk direction, and e is of the order 
of machine epsilon. This method is preferable to a finite-difference approximation, 
since it does not suffer from round-off errors, which would otherwise limit how small 
e can be. 

In our numerical simulation, we computed an optimal trajectory for a rest-to- 
rest maneuver, as illustrated in Figure (2). Here, the maneuver time is 12.8sec, 
N — 128, and the moment of inertia is given by 

13.25 -7.80 -11.40' 
-7.80 16.25 4.71 
-11.40 4.71 18.37 

The prescribed maneuver corresponds to a rotation by ^ about the x-axis. Since 
the moment of inertia tensor is not a multiple of the identity, and the x-axis does 
not correspond to the axis of minimal inertia, the optimal trajectory does not just 
involve a pure rotation about the x-axis. It is worth noting that the results are not 
rotationally symmetric about the midpoint of the simulation interval, which is due 
to the fact that our choice of update, Rfc+i = Rfc exp(/ir2fc), does not exhibit time- 
reversal symmetry. In a forthcoming publication, we will introduce a reversible 
algorithm to address this issue. In particular, this will involve explicitly computing 
the stationarity conditions for the discrete optimal control problem constrained by 
the time-symmetric Lie Stormer-Verlet method. 

We also present results for an optimal slew-up maneuver, illustrated in Figure 
(3). This uses the same moment of inertia tensor as in the previous simulation, and 
the desired maneuver involves a rotation of ^ about the x-axis from rest to a final 
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angular velocity ofri7v-i=[0-3 0.2 0.3] , over a maneuver time of 12.8sec, and 
N ^ 128. 

6. Conclusion 

In this paper we studied the continuous- and discrete-time optimal control prob- 
lem for the rigid body, where the cost to be minimized is the external torque applied 
to move the rigid body from an initial condition to some pre-specified terminal con- 
dition. In the discrete setting, we use the discrete Lagrange-d'Alembert principle 
to obtain the discrete equations of motion. The kinematics were discretized to guar- 
antee that the flow in phase space remains on the Lie group S0(3) and its algebra 
so(3). We described how the necessary conditions can be solved for the general 
three-dimensional case and gave a numerical example for a three-dimensional rigid 
body maneuver. 

The synthesis of variational mechanics with discrete-time optimal control is par- 
ticularly advantageous from the point of view of computational efficiency, since the 
symplectic Euler method is symplectically conjugate to the Stormer-Verlet method, 
and hence has effective order two. Consequently, for our discrete-time optimal con- 
trol method, the cost functional converges at a rate which is characteristic of a 
second-order method, while being based on a first-order method that is computa- 
tionally cheaper. 

Currently, we are investigating the use of the Pontryagin's maximum principle 
with Lie group methods in continuous- and discrete-time to obtain the necessary 
conditions. Additionally, we wish to generalize the result to general Lie groups that 
have applications other than the rigid body motion on S0(3). In particular, we are 
interested in controlling the motion of a rigid body in space, which corresponds to 
motion on the non-compact Lie group SE(3). 
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(c) Instantaneous rotation axis 

Figure 2. Discrete optimal rest-to-rest maneuver in S0(3). 
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(c) Instantaneous rotation axis 

Figure 3. Discrete optimal slew-up maneuver in S0(3). 



